#include "./move_motor.h"
#include "../../device/gpio.h"
#include "../../device/timer.h"
#include "../../utils/progress.h"
#include "./sensor.h"

namespace driver {

using namespace device::gpio;
using namespace device::timer;
using namespace utils;

using PULS = PA<1>;
using DIR = PC<3>;
using ENABLE = PC<5>;
using ALM = PC<4>;

using PWM = PWMTimer<TIM2_BASE, 72 - 1, 2>;

static Error puls_once() {
    PULS::set();
    osDelay(1);
    PULS::clr();
    osDelay(1);
    if (MoveMotor::is_alm()) {
        return ec::POWDER_MOVE_MOTOR_LOCKED;
    }
    return ec::OK;
}

static Error puls_multi(uint32_t count) {
    Error err;
    for (uint32_t i = 0; i < count; i ++) {
        err = puls_once();
        if (err) {
            break;
        }
    }
    return err;
}

MoveMotor::MoveMotor() {
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN |
        RCC_APB2ENR_IOPCEN;
    
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;

    config_pp_out<PULS, DIR, ENABLE>();
    config_float_input<ALM>();
    set_dir(Dir::LEFT);
    notify();
    PWM::stop();
    NVIC_EnableIRQ(TIM2_IRQn);
}

bool MoveMotor::is_alm() {
    return !ALM::read();
}

void MoveMotor::set_dir(Dir dir) {
    switch (dir) {
        case Dir::RIGHT:
            ENABLE::clr();
            DIR::set();
        break;

        case Dir::LEFT:
            ENABLE::clr();
            DIR::clr();
        break;

        case Dir::RELEASE:
            ENABLE::set();
        break;
    }
}

void MoveMotor::disable_pwm() {
    PWM::stop();
}

void MoveMotor::set_pwm_freq(uint32_t freq) {
    PWM::set_pwm_freq(freq);
}

void MoveMotor::enable_pwm(uint32_t base_freq) {
    config_af_pp_out<PULS>();
    PWM::start_with_irq(base_freq);
}

void MoveMotor::enable_pwm_not_irq(uint32_t freq) {
    config_af_pp_out<PULS>();
    PWM::start_with_no_irq(freq);
}

Error MoveMotor::reset_adj() {
    Error err;
    uint32_t puls_count = 0;

    config_pp_out<PULS>();
    if (sensor->is_move_origin()) {
        set_dir(Dir::LEFT);
        for (;;) {
            if (!sensor->is_move_origin()) {
                break;
            }
            err = puls_once();
            if (err) {
                goto finish;
            }
            puls_count ++;
            if (puls_count > ADJ_MAX_COUNT) {
                err = ec::POWDER_MOVE_MOTOR_TIMEOUT;
                goto finish;
            }
        }
        puls_multi(10);
    }
    puls_count = 0;
    set_dir(Dir::RIGHT);
    for (;;) {
        if (sensor->is_move_origin()) {
            break;
        }
        err = puls_once();
        if (err) {
            goto finish;
        }
        puls_count ++;
        if (puls_count > ADJ_MAX_COUNT) {
            err = ec::POWDER_MOVE_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    return err;
}

Error MoveMotor::reset() {
    Error err;
    Progress prog;

    if (!sensor->is_move_origin()) {
        set_dir(Dir::RIGHT);
        enable_pwm_not_irq(RESET_FREQ);
        for (;;) {
            if (sensor->is_move_origin()) {
                break;
            }

            if (is_alm()) {
                err = ec::POWDER_MOVE_MOTOR_LOCKED;
                goto finish;
            }
        
            if (prog.is_after(RESET_TIMEOUT)) {
                err = ec::POWDER_MOVE_MOTOR_TIMEOUT;
                goto finish;
            }

            osDelay(20);
        }
        disable_pwm();
    }
    osDelay(1000);
    err = reset_adj();
finish:
    disable_pwm();
    m_reset_flag = err.is_ok();
    return err;
}

Error MoveMotor::async_run(uint16_t position) {
    Error err;
    if (!m_reset_flag) {
        err = reset();
        if (err) {
            return err;
        }
    }
    await();
    if (m_position == position) {
        notify();
        return err;
    }

    if (position > m_position) {
        set_dir(Dir::LEFT);
        run_motor(position - m_position);
    } else {
        set_dir(Dir::RIGHT);
        run_motor(m_position - position);
    }
    m_position = position;
    return err;
}

Error MoveMotor::wait_for() {
    Error err;

    await();

    if (is_alm()) {
        err = ec::POWDER_MOVE_MOTOR_LOCKED;
        goto finish;
    }

    if (m_position == 0) {
        osDelay(1000);
        err = reset_adj();
    }

finish:
    m_reset_flag = err.is_ok();
    notify();
    return err;
}

Error MoveMotor::run(uint16_t position) {
    Error err = async_run(position);
    if (err) {
        return err;
    }
    return wait_for();
}

}

using namespace driver;

extern "C"
void TIM2_IRQHandler() {
    if (TIM2->SR & TIM_SR_UIF) {
        TIM2->SR &= ~TIM_SR_UIF;
        moveMotor->pwm_epoch_update();        
    }
}


